#include "Planner.h"
#include <CGAL/minkowski_sum_2.h>
#include <CGAL/point_generators_2.h>
#include "Kd_tree_d.h"
#include <boost/make_shared.hpp>
#include <CGAL/Boolean_set_operations_2.h>
#include <boost/foreach.hpp>
#include "PrmPlanner.h"

using namespace std;

//TODO: set the correct price
const int Planner::GAP_EXT_PRICE = 10;

Planner::Planner(Scene* scene) : m_scene(scene)
{
    /*	this method extracts information regarding the scenario from the 
        gui and initializes the fields 
            m_robot_polygons
            m_obstacles
            m_start_confs, m_target_confs
            m_room
            m_robot_num
    */
    extract_scenario_from_gui();   
    m_hgraph = new Graph<PointDPtr>(0, true);

}

Planner::~Planner()
{
}

/*	This function is invoked by the GUI and is assumed to update the resulting */
void Planner::run()
{
    //generate paths using prm
    vector<MPATH> paths = generatePrmPaths(3);

    //the vertices of the hgraph are all the nodes in paths.
    //the edges include the previous edges
    //the start and the target in all the paths are the same
    PointDPtr start = vectorConfToPointD(paths[0][0]);
    PointDPtr target = vectorConfToPointD(paths[0][paths[0].size()-1]);
    m_hgraph->add_vertex(start);
    m_hgraph->add_vertex(target);
    BOOST_FOREACH(MPATH path, paths){
        PointDPtr prev = start;
        for(int i = 1; i <path.size()-1; ++i){
            PointDPtr p = vectorConfToPointD(path[i]);
            m_hgraph->add_vertex(p);
            //TODO: add weight
            m_hgraph->add_edge(prev, p);
            prev = p;
        }
        //TODO: add weight
        m_hgraph->add_edge(prev, target);
    }
    
    //combine paths
    for(int i=0; i<paths.size(); ++i){
        for(int j=i; j<paths.size(); ++j){
            vector<vector<PointDPtr>> potentialBridgeEdges = getPotentialBridgeEdges(paths[i], paths[j]);
            BOOST_FOREACH(vector<PointDPtr> edge , potentialBridgeEdges){
                bool isValid = localPlanner(edge[0], edge[1]);        
                if (isValid){
                    //TODO: calculate the real weight
                    int distance = 1;
                    int clearance = 1;
                    int alfa = 1;
                    double weigt = distance/(clearance ^ alfa);
                    m_hgraph->add_edge(edge[0], edge[1], weigt);
                }
            }
        }
    }    

    //run dijkstra
    list<PointDPtr> bestPath;
    m_hgraph->find_path(start, target, bestPath);

    //translate to configuration space
    BOOST_FOREACH(PointDPtr p, bestPath){
        m_path.push_back(m_pointdptr_to_point2[p]);
    }

    //	run this method when you finish to produce the path
    //	IMPORTANT: the result should be put in m_path
    transform_path_for_gui(m_robot_num, m_path, m_scene);
}

bool Planner::localPlanner(PointDPtr from, PointDPtr to){
   //TODO: use local planner for two robots: for example prm
    return false;
}



vector<vector<PointDPtr>> Planner::getPotentialBridgeEdges(MPATH p , MPATH q){
    vector<vector<PointDPtr>> bridges;
    map<pair<int,int>,int> costMatrix;
    map<pair<int,int>,MOVEMENT> traceBackMatrix;

    for(int i=0; i<p.size(); ++i){
        for(int j=0; j<q.size(); ++j){
            int _match = getDelta(vectorConfToPointD(p[i]), vectorConfToPointD(q[j]));
            if( i!=0 && j!=0){
                _match += costMatrix.at(make_pair(i-1, j-1));
            }

            int _up = GAP_EXT_PRICE;
            if(i != 0){
                _up += costMatrix.at(make_pair(i-1, j));
            }

            int _left = GAP_EXT_PRICE;
            if( j != 0 ){
                _left += costMatrix.at(make_pair(i, j-1));
            }

            if(_match < _up){
                if (_left < _match){
                    traceBackMatrix[make_pair(i,j)] = LEFT;
                    costMatrix[make_pair(i,j)] = _left;
                } else{
                    traceBackMatrix[make_pair(i,j)] = MATCH;
                    costMatrix[make_pair(i,j)] = _match;
                }
            } else{
                if(_left < _up){
                    traceBackMatrix[make_pair(i,j)] = LEFT;
                    costMatrix[make_pair(i,j)] = _left;
                } else{
                    traceBackMatrix[make_pair(i,j)] = UP;
                    costMatrix[make_pair(i,j)] = _up;
                }
            }
        }

        // trace back and get all the additional edges
        bool isGapUp = false;
        bool isGapLeft = false;
        int i = p.size()-1;
        int j = q.size()-1;
        while(i!=0 && j!=0){
            MOVEMENT mov = traceBackMatrix[make_pair(i, j)];
            switch(mov){
            case(MATCH):
                {
                    vector<PointDPtr> edge1;
                    edge1[0] = vectorConfToPointD(p[i]);
                    edge1[1] = vectorConfToPointD(q[j]);
                    bridges.push_back(edge1);

                    if(isGapUp){
                        isGapUp = false;
                        vector<PointDPtr> edge2;
                        edge2[0] = vectorConfToPointD(p[i]);
                        edge2[1] = vectorConfToPointD(q[j-1]);
                        bridges.push_back(edge2);
                    }

                    if (isGapLeft){
                        isGapLeft = false;
                        vector<PointDPtr> edge3;
                        edge3[0] = vectorConfToPointD(p[i-1]);
                        edge3[1] = vectorConfToPointD(q[j]);
                        bridges.push_back(edge3);
                    }

                    if(i!=0)
                        i--;
                    if(j!=0)
                        j--;
                    break;
                }
            case(UP):
                {
                    if(! isGapUp){
                        isGapUp = true;
                        vector<PointDPtr> edge4;
                        edge4[0] = vectorConfToPointD(p[i-1]);
                        edge4[1] = vectorConfToPointD(q[j]);
                        bridges.push_back(edge4);
                    }
                    if( isGapLeft ){
                        //TODO: exit left gap and enter up gap
                    }
                    if(i!=0)
                        i--;
                    break;
                }
            case(LEFT):
                {
                    if (! isGapLeft){
                        isGapLeft = true;
                        vector<PointDPtr> edge5;
                        edge5[0] = vectorConfToPointD(p[i]);
                        edge5[1] = vectorConfToPointD(q[j-1]);
                        bridges.push_back(edge5);
                    }
                    if( isGapUp ){
                        //TODO: exit up gap and enter left gap
                    }
                    if(j!=0){
                        j--;
                    }
                    break;
                }
            default: //we shouldnt be here..
                break;
            }
            
        }


    } 

    return bridges;
}

int Planner::getDelta(PointDPtr p, PointDPtr q){
    //TODO
    return 0;
}

PointDPtr Planner::vectorConfToPointD(vector<Conf> vec){
    std::vector< double > coord;

    BOOST_FOREACH(Conf& point, vec){
        coord.push_back(point.x().exact().to_double());
        coord.push_back(point.y().exact().to_double());
    }

    PointDPtr point_d(new Point_d(m_robot_num * 2, coord.begin(), coord.end()));
    m_pointdptr_to_point2[point_d] = vec;
    return point_d;
}

vector<MPATH> Planner::generatePrmPaths(int numOfPaths){
    vector<MPATH> prmPaths;

    for(int i=0; i<numOfPaths; ++i){
        PrmPlanner prmPlanner(m_room, m_robot_polygons, m_obstacles, m_robot_num, m_start_confs, m_target_confs);
        prmPlanner.run();
        prmPaths.push_back(prmPlanner.getPath());        
    }
    return prmPaths;
}

